#include "histograma.h"

histograma::histograma()
{
    for (int i = 0; i < 40; i++) {
        valoresHistograma[i] = (float)1/40;
    }
}

float histograma::getPosicionLinea(uint32_t *valoresBarras, float desplazamiento)
{
    int32_t medidaPonderada[24];
    int32_t mediaValores = 0;
    float histogramaTotal = 0;
    float expandedHistogram[24];


    for (int i = 0; i < 24; i++) {

        if ((i >= 4) && (i <= 19)) {
            medidaPonderada[i] = (int32_t)((valoresBarras[i] + valoresBarras[i+20])/2);
        }else {
            medidaPonderada[i] = valoresBarras[i];
        }

        //Contribuir para la media
        mediaValores += medidaPonderada[i];

        //Vaciamos el vector de histograma expandido
        expandedHistogram[i] = 0;
    }

    //Dividimos para obtener la media
    mediaValores /= 24;

    //Aplicamos incertidumbre de movimiento
    for (int i = 0; i < 24; i++) {

        if (i > 1) {
            expandedHistogram[i - 2] += valoresHistograma[i]*0.083;
        }

        if (i > 0) {
            expandedHistogram[i - 1] += valoresHistograma[i]*0.167;
        }

        expandedHistogram[i] += valoresHistograma[i]*0.5;

        if (i < 23) {
            expandedHistogram[i + 1] += valoresHistograma[i]*0.167;
        }

        if (i < 22) {
            expandedHistogram[i + 2] += valoresHistograma[i]*0.083;
        }

    }

    //Multiplicamos por probabilidad
    for (int i = 0; i < 24; i++) {
        float probabilidad = generarConfianza(medidaPonderada, i, mediaValores);

        valoresHistograma[i] = expandedHistogram[i] * probabilidad;
        histogramaTotal += valoresHistograma[i];
    }

    //Normalizamos las probabilidades
    indexMaxHist = 0;
    for (int i = 0; i < 24; i++ ){
        valoresHistograma[i] /= histogramaTotal;

        if (valoresHistograma[i] > valoresHistograma[indexMaxHist]) {
            indexMaxHist = i;
        }
    }

    //Calculamos la media
    posicionLinea = 0;

    for (int i = 0; i < 24; i++) {
        posicionLinea += i*valoresHistograma[i];
    }

    hayLinea = (valoresHistograma[indexMaxHist] > 0.2);

    return posicionLinea;
}

float histograma::getPosicionLinea2(uint32_t * valoresBarras, float angulo, float velocidad, float periodo)
{
    float expandedHistogram[40];
    float medidaPonderada[24];
    float histogramaTotal = 0;
    float dx = angulo * velocidad * periodo * LEDS_METRO / 1000 / 100; //El periodo esta en ms y el angulo va de -100 a 100
    float error = 5 * velocidad * periodo * LEDS_METRO / 1000 / 100; //El error en grados (grados ficticios -100 a 100) que tienen nuestros servos

    for (int i = 0; i < 24; i++) {

        //Tener en cuenta los LEDS que consideramos inútiles
        if ((i >= 4) && (i <= 19)) {
            medidaPonderada[i] = (valoresBarras[i] + valoresBarras[i+20])/2;
        }else {
            medidaPonderada[i] = valoresBarras[i];
        }

        //Vaciamos el vector de histograma expandido
        expandedHistogram[i] = 0;
    }

    //Calculamos la expansión necesaria debida al movimiento
    float a,b,c; // variables de expansión para indices relativos -1, 0, 1
    float startb, endb;

    if (error > dx) {
            a = (error - dx) / (1 + 2*error);
            startb = 0;
    }else {
        a = 0;
        startb = dx - error;
    }

    if ((error + dx) > 1) {
        c = (error + dx) / (1 + 2*error);
        endb = 1;
    }else {
        c = 0;
        endb = dx + error;
    }

    b = (endb - startb) / (1 + 2*error);

    //Aplicamos la expansión de histograma
    for (int i = 0; i < 40; i++) {

        if (i > 0) {
            expandedHistogram[i - 1] += valoresHistograma[i]*a;
        }

        expandedHistogram[i] += valoresHistograma[i]*b;

        if (i < 39) {
            expandedHistogram[i + 1] += valoresHistograma[i]*c;
        }
    }

    //Multiplicamos por probabilidad
    for (int i = 0; i < 40; i++) {
        float probabilidad;

        if ((i < 8) || (i >= 32)) {
            probabilidad = 1;
        }else {
            probabilidad = generarConfianza2(medidaPonderada, i-8);
        }

        valoresHistograma[i] = expandedHistogram[i] * probabilidad;
        histogramaTotal += valoresHistograma[i];
    }

    //Normalizamos las probabilidades
    indexMaxHist = 0;
    for (int i = 0; i < 40; i++ ){
        valoresHistograma[i] /= histogramaTotal;

        if (valoresHistograma[i] > valoresHistograma[indexMaxHist]) {
            indexMaxHist = i;
        }
    }

    int numLedsLinea = 0;
    posicionLinea = 0;

    for (int i = -2; i <= 2; i++) {
        if ((indexMaxHist + i) < 0) {
            continue;
        }else if ((indexMaxHist + 1) >= 40) {
            break;
        }

        posicionLinea += valoresHistograma[indexMaxHist + i] * (indexMaxHist + i);
        numLedsLinea++;
    }

    posicionLinea /= numLedsLinea;

    return posicionLinea;
}

float *histograma::getHistograma()
{
    return valoresHistograma;
}

float histograma::generarConfianza2(float *medidaPonderada, int i)
{
    float m = ((float)(PROB_MULT_LEDS*PROB_MULT_LEDS - 1))/(PROB_MULT_LEDS * (calibracionBlanco[i] - calibracionNegro[i]));
    float n = PROB_MULT_LEDS - m*calibracionBlanco[i];
    float prob = m*medidaPonderada[i]+n;

    prob = (prob > PROB_MULT_LEDS) ? PROB_MULT_LEDS : prob;
    prob = (prob < ((float)1/PROB_MULT_LEDS)) ? ((float)1/PROB_MULT_LEDS) : prob;

    return prob;
}

float histograma::generarConfianza(int32_t *valoresBarras, int indice, int32_t media)
{
    float confianza = (float)(media-valoresBarras[indice]);

    if (indice > 0) {
        confianza +=  (float)(media-valoresBarras[indice-1])/2;
    }

    if (indice < 24) {
        confianza +=  (float)(media-valoresBarras[indice+1])/2;
    }

    return (confianza > (float)0.1) ? confianza :(float)0.1;
}
